/*----------------------------------------------------------------------------*/
/* Team 3647
/* DriveBase.java
/* Has all driving functions and bridge lowering
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;

public class DriveBase {

    public static Joystick Stick1 = new Joystick(1);
    private static LinearVictor VictorLeft = new LinearVictor(1);
    private static LinearVictor VictorRight = new LinearVictor(2);
    private static final Solenoid BridgeDown = new Solenoid(2);
    private static final Solenoid BridgeUp = new Solenoid(8);
    private static final Solenoid ArmStop = new Solenoid(7);
    
    /***************************************************************************
     * Drive function based off two joysticks
     * Uses the Cheesy Drive function.
     ***************************************************************************/
    public static void TankCD() {
        double LeftJoyStick  = Stick1.getRawAxis(2);
        double RightJoyStick = Stick1.getRawAxis(4);

        double throttle = (LeftJoyStick + RightJoyStick) / 2;
        double wheel    = (LeftJoyStick - RightJoyStick) / 2;
        
        if (Stick1.getRawButton(8)) {
            throttle = -.65;
        } else if (Stick1.getRawButton(7)) {
            throttle = .65; 
        }

        
        if (Math.abs(throttle) <= 0.3 && Math.abs(throttle) >= 0.05) 
            throttle = 0.3 * (throttle < 0 ? -1 : 1);
        else if (Math.abs(throttle) < 0.05) 
            throttle = 0;
        CheesyDrive(throttle, wheel, true);
    }
   
    public static void CheesyDrive(double throttle, double wheel, boolean quickTurn) {
        double angular_power = 0.0;
        double overPower = 0.0;
        double sensitivity = 1.5;
        double rPower = 0.0;
        double lPower = 0.0;
        if (quickTurn) {
            overPower = .25;
            sensitivity = .75;
            angular_power = wheel;
        } else {
            overPower = 0.0;
            angular_power = Math.abs(throttle) * wheel * sensitivity;
        }
        rPower = lPower = throttle;
        lPower += angular_power;
        rPower -= angular_power;
        if (lPower > 1.0) {
            rPower -= overPower * (lPower - 1.0);
            lPower = 1.0;
        } else if (rPower > 1.0) {
            lPower -= overPower * (rPower - 1.0);
            rPower = 1.0;
        } else if (lPower < -1.0) {
            rPower += overPower * (-1.0 - lPower);
            lPower = -1.0;
        } else if (rPower < -1.0) {
            lPower += overPower * (-1.0 - rPower);
            rPower = -1.0;
        }
        Drive(lPower, rPower);
    }

    /***************************************************************************
     * Drive function
     * Left and Right
     ***************************************************************************/
    public static void Drive(double left, double right) {
        VictorLeft.set(-left);
        VictorRight.set(right);
    }
}
